#include <math.h>
#include "pid.h"
#include "my_ano.h"

/*X,Y轴的PID结构体*/
_pid_t  pid_X,pid_Y;


/** @brief 单个pid结构体的3个参数设置
 *  @param 
 *  @retval 无
 */
void set_p_i_d(_pid_t* pidStruct,float Kp, float Ki, float Kd)
{
	pidStruct->Kp=Kp;
	pidStruct->Ki=Ki;
	pidStruct->Kd=Kd;
}

/** @brief 单个pid结构体的目标值设置
 *  @param 
 *  @retval 无
 */
void set_pid_target(_pid_t* pidStruct,float target_val)
{
	pidStruct->target_val=target_val;
}

/** @brief 单个pid结构体的目标值获取
 *  @param 
 *  @retval 无
 */
float get_pid_target(_pid_t* pidStruct)
{
	return pidStruct->target_val;
}

/** @brief 单个pid结构体的赋值初始化
 *  @param 
 *  @retval 无
 */
void pidStruct_init(_pid_t* pidStruct,float target_val,float Kp, float Ki, float Kd)
{
	set_pid_target(pidStruct,target_val);
	set_p_i_d(pidStruct,Kp,Ki,Kd);
		
	pidStruct->actual_val=0;
    pidStruct->err=0;
    pidStruct->err_last=0;
    pidStruct->integral=0;
	pidStruct->pid_status=0;
}

/** @brief 单个pid结构体的算法实现
 *  @param 
 *  @retval 无
 *  @description:
	根据位置式离散PID公式 
	pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
	e(k)代表本次偏差 
	e(k-1)代表上一次的偏差  
	∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
 */
float pid_realize(_pid_t* pidStruct)
{
    pidStruct->err=pidStruct->actual_val-pidStruct->target_val;			//计算真实值与目标值的误差

    pidStruct->integral+=pidStruct->err;

    pidStruct->output=(int)(pidStruct->Kp*pidStruct->err+pidStruct->Ki*pidStruct->integral+pidStruct->Kd*(pidStruct->err-pidStruct->err_last));

    pidStruct->err_last=pidStruct->err;

    return pidStruct->output;
}
